/*
 * Created on 01 decembre 2007
 * cedric.hartland(at)lri.fr
 *
 * This is demo for loading a map of a room with chairs and tables from an image file in Simbad. 
 * 
 */
package contribs.roomloader;

import java.io.Serializable;
import java.util.ArrayList;
import java.util.Random;

import javax.vecmath.Color3f;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import contribs.maploader.SimpleImage;

import simbad.gui.Simbad;
import simbad.sim.Agent;
import simbad.sim.BallAgent;
import simbad.sim.EnvironmentDescription;
import simbad.sim.RangeSensorBelt;
import simbad.sim.RobotFactory;
import simbad.sim.Wall;

/**
 * This class represents an Environment useable in Simbad but with the 
 * specification of being defined by a PNG image. For the moment, the image
 * is defined as follows
 *  - 20x20 pixels (have to fit)
 *  - a red (rgb 255,0,0) pixel means starting point for a robot
 *  - a green (rgb 0,255,0) pixel means a table
 *  - a blue (rgb 0,0,255) pixel means a chair
 *  - black (rgb 0,0,0) pixel means a wall
 *  - the image have to be in a PNG, JPG or GIF format.
 * 
 * @author cedric ( + nicolas )
 *
 */
public class RoomLoaderDemo extends EnvironmentDescription implements Serializable {
	
	
	//static private Simbatch sim;
	//private Robot robot;
	
	// a goal to reach
	public Point3d _goal = new Point3d();
	// the initial starting points for robots (contains Point3d)
	public ArrayList _startingPoints = new ArrayList();
	
	
	public RoomLoaderDemo() 
	{
		// build the environment
		// here a simple square-closed environment.
		Wall w1 = new Wall(new Vector3d(9, 0, 0), 19, 1, this); w1.rotate90(1); add(w1);
		Wall w2 = new Wall(new Vector3d(-9, 0, 0), 19, 2, this); w2.rotate90(1); add(w2);
		Wall w3 = new Wall(new Vector3d(0, 0, 9), 19, 1, this); add(w3);
		Wall w4 = new Wall(new Vector3d(0, 0, -9), 19, 2, this); add(w4);
		
		// create the robot
		
		add(new Robot(new Vector3d(0, 0, 0), "robot"));
		
	}
	
	public RoomLoaderDemo( String __filename,boolean optimized ) 
	{
		optimizedImageLoader(__filename);
	}
	
	/**
	 * initialize the environment due to the file __filename.
	 * TODO : 
	 *  - create multi-objectives points
	 *  - create balls or non static items. 
	 * @param __filename
	 */
	public RoomLoaderDemo( String __filename ) 
	{
		int [] values;
		
		// load the image
		SimpleImage simpleImage = new SimpleImage (__filename, false);
		simpleImage.displayInformation();
		
		// step 2 : for each data in the image, initialize the environment
		//          + starting position (for one or more robots)
		//          + goal position
		for ( int y = 0 ; y != simpleImage.getHeight() ; y++ )
		{
			System.out.print(" ");
			for ( int x = 0 ; x != simpleImage.getWidth() ; x++ )
			{
				values = simpleImage.getPixel(x,y); 			
				if ( values[1] > 200 && values[2] < 50 && values[3] < 50 ) 
				{
					// red value, we will display a Wall in here : 
					add(new Wall(new Vector3d(x-(simpleImage.getWidth()/2),0,y-(simpleImage.getHeight()/2)),1,1,1,this));
					System.out.print("#");
				}
				else
					if ( values[1] < 50 && values[2] > 200 && values[3] < 50 )
					{
						// green value : define the goal point
						_goal.x = x; _goal.z = y; _goal.y = 0;
						System.out.print("X");
					}
					else
						if ( values[1] < 50 && values[2] < 50 && values[3] > 200 )
						{
							_startingPoints.add(new Point3d(x,0,y));
							// starting position
							System.out.print("!");
						}
						else
							if (values[1]<100 && values[1]==values[2] && values[2]==values[3]){
								// add a ball
								// take care because the setUsePhysics remove the agentInspector
								showAxis(false);
								setUsePhysics(true);				            
								add(new BallAgent(new Vector3d(x-(simpleImage.getWidth()/2), 0, y-(simpleImage.getHeight()/2)), "ball", new Color3f(200,0,0),0.25f,0.25f));
								
							}
							else
								System.out.print(" ");
			}
			System.out.print("\n");
		}
		
		// add the robots of Robot instances
		for(int i=0;i<_startingPoints.size();i++){
			add(new Robot(new Vector3d(((Point3d)_startingPoints.get(i)).x-(simpleImage.getWidth()/2), 0f, ((Point3d)_startingPoints.get(i)).z-(simpleImage.getHeight()/2)), "openDProbot"));
		}
	}
	
	
	/**
	 * This loader does not consider methodic exploration of the image but 
	 * search for features (ie walls from (x,y) to (x2,y2)) and different objects
	 * of different sizes and orientations.
	 * It takes much more time to build but provide a lighter environment which
	 * is better for evolutionnary computations.
	 * @param __filename
	 */
	public void optimizedImageLoader(String __filename){
		// load the image
		SimpleImage simpleImage = new SimpleImage (__filename, false);
		simpleImage.displayInformation();
		
		searchWalls(simpleImage);
		searchTables(simpleImage);
		searchChairs(simpleImage);
		searchRobots(simpleImage);
	}
	
	/**
	 * a wall is defined by a black line
	 * @param source
	 */
	private void searchWalls(SimpleImage source){
		int [] values;
		int xx =0, xxx=0;
		int yy=0, yyy=0;
		wallColor=new Color3f(0.6f,0.5f,0.6f);
		for ( int y = 0 ; y != source.getHeight() ; y++ ){
			for ( int x = 0 ; x != source.getWidth() ; x++ ){
				values = source.getPixel(x,y);
				// dark/black value -> explore to find a wall
				if (values[1]<10 & values[2]<10 & values[3]<10){
					xx=x;
					yy=y;
					// compute k the higher value possible
					int k = 0;
					boolean limit = false;
					while(!limit){
						k++;
						if ((x+k)<source.getWidth()){
							int [] valuesTemp = source.getPixel(x+k,y);
							if (!(valuesTemp[1]==values[1] && 
									valuesTemp[2]==values[2] &&
									valuesTemp[3]==values[3]))
								limit=true;
							else {
								boolean neighbor = false;
								// check neighbors to remove the pixel or not
								if (y-1>=0) {
									valuesTemp = source.getPixel(x+k,y-1);
									if (valuesTemp[1]==values[1] && 
											valuesTemp[2]==values[2] &&
											valuesTemp[3]==values[3]) neighbor=true;
								}
								if (y+1<source.getHeight()) {
									valuesTemp = source.getPixel(x+k,y+1);
									if (valuesTemp[1]==values[1] && 
											valuesTemp[2]==values[2] &&
											valuesTemp[3]==values[3]) neighbor=true;
								}
								if (!neighbor) source.setPixel(x+k,y,255,255,255,255);
							}
						} else 	limit=true;
					}
					//if ((x+k)<source.getWidth()){source.setPixel(x+k,y,values[0],values[1],values[2],values[3]);}
					// if the wall is horizontal !!
					if (k>1) {
						xxx=xx+k;
						add(new Wall(new Vector3d(x+(xxx-x)/2d-(source.getWidth()/2d),0,y+0.5-(source.getHeight()/2d)),Math.abs(xxx-xx)-0.7f,0.3f,2,this));
						System.out.println("add a wall "+(xxx-xx));
					}
				}
			}
		}
		
		for ( int y = 0 ; y != source.getHeight() ; y++ ){
			for ( int x = 0 ; x != source.getWidth() ; x++ ){
				values = source.getPixel(x,y);
				// dark/black value -> explore to find a wall
				if (values[1]<10 & values[2]<10 & values[3]<10){
					xx=x;
					yy=y;
					// compute k the higher value possible
					// the wall might be vertical
					int k = 0;
					boolean limit = false;
					while(!limit){
						k++;
						if ((y+k)<source.getHeight()){
							int [] valuesTemp = source.getPixel(x,y+k);
							if (!(valuesTemp[1]==values[1] && 
									valuesTemp[2]==values[2] &&
									valuesTemp[3]==values[3]))
								limit=true;
							else source.setPixel(x,y+k,255,255,255,255);
						} else 	limit=true;
					}
					//if ((y+k)<source.getHeight()){source.setPixel(x+k,y,values[3],values[0],values[1],values[2]);}
					
					// if the wall is horizontal !!
					if (k>1) {
						yyy=yy+k;
						add(new Wall(new Vector3d(x+0.5d-(source.getWidth()/2d),0,y+(yyy-y-source.getHeight())/2d),0.3f,Math.abs(yyy-yy)-0.7f,2,this));
						System.out.println("add a wall "+(yyy-yy));
					}
					
				}
			}
		}
	}
	
	
	private void searchTables(SimpleImage source){
		int [] values;
		int yy=0;
		int xx=0;
		for ( int y = 0 ; y != source.getHeight() ; y++ ){
			for ( int x = 0 ; x != source.getWidth() ; x++ ){
				values = source.getPixel(x,y);
				if ( values[1] < 50 && values[2] > 200 && values[3] < 50 ){
					yy=0;
					xx=0;
					// check for the size of the table
					boolean endline=false;
					while(!endline){
						yy++;
						if ((y+yy)<source.getHeight()){
							int [] valuesTemp = source.getPixel(x,y+yy);
							if (!(valuesTemp[0]==values[0] && 
									valuesTemp[1]==values[1] &&
									valuesTemp[2]==values[2]))
								endline=true;
						}							
						else endline=true;
					}
					endline=false;
					while(!endline){
						xx++;
						if ((x+xx)<source.getWidth()){
							int [] valuesTemp = source.getPixel(x+xx,y);
							if (!(valuesTemp[0]==values[0] && 
									valuesTemp[1]==values[1] &&
									valuesTemp[2]==values[2]))
								endline=true;
						}							
						else endline=true;
					}
					System.out.println("add a table at ("+x+","+y+") with "+xx+","+yy);
					add(new Table(new Vector3d(x-(-xx+source.getWidth())/2d,0,y-(-yy+source.getHeight())/2d),0.22f,yy*5,xx*5,this));
					// clean the zone in the image :
					for(int i=x;i<x+xx;i++){
						for(int j=y;j<y+yy;j++){
							source.setPixel(i,j,255,255,255,255);
						}
					}
					
				}
			}
		}
	}
	
	private void searchChairs(SimpleImage source){
		int [] values;
		Random r = new Random();
		for ( int y = 0 ; y != source.getHeight() ; y++ ){
			for ( int x = 0 ; x != source.getWidth() ; x++ ){
				values = source.getPixel(x,y);
				if ( values[1] < 50 && values[2] < 50 && values[3] > 200 ){
					Chair ch2 = new Chair(new Vector3d(x+0.5-source.getWidth()/2d,0,y+0.5-source.getWidth()/2d),0.22f,this);
					ch2.rotateY(r.nextDouble()*(2*Math.PI));
					add(ch2);
				}
			}
		}
	}
	
	private void searchRobots(SimpleImage source){
		int [] values;
		Random r = new Random();
		for ( int y = 0 ; y != source.getHeight() ; y++ ){
			for ( int x = 0 ; x != source.getWidth() ; x++ ){
				values = source.getPixel(x,y);
				if ( values[1] > 200 && values[2] < 50 && values[3] < 50 ){
					Robot r2d2 = new Robot(new Vector3d(x+0.5-source.getWidth()/2d,0,y+0.5-source.getWidth()/2d), "testrobot");
					add(r2d2);

				}
			}
		}
	}
	

	  public class Robot extends Agent {

	        RangeSensorBelt sonars, bumpers;

	        public Robot(Vector3d position, String name) {
	            super(position, name);
	            // Add sensors
	            bumpers = RobotFactory.addBumperBeltSensor(this);
	            sonars = RobotFactory.addSonarBeltSensor(this,24);
	        }

	        /** Initialize Agent's Behavior */
	        public void initBehavior() {
	        // nothing particular in this case
	        }

	        /** Perform one step of Agent's Behavior */
	        public void performBehavior() {
	            
	            if (bumpers.oneHasHit()) {
	                setTranslationalVelocity(-0.1);
	                setRotationalVelocity(0.1 * Math.random());

	            } else {
	                // Front left obstacle ?
	                if (sonars.hasHit(0) && (sonars.hasHit(1))) {
	                    setRotationalVelocity(-Math.PI / 4);
	                    setTranslationalVelocity(0.1);
	                }
	                // Front right obstacle ?
	                else if (sonars.hasHit(0) && (sonars.hasHit(23))) {
	                    setRotationalVelocity(Math.PI / 4);
	                    setTranslationalVelocity(0.1);
	                }
	                // left obstacle ?
	                else if (sonars.hasHit(3) || sonars.hasHit(4)) {
	                    setRotationalVelocity(-Math.PI / 8);
	                    setTranslationalVelocity(0.1);
	                }
	                // right obstacle ?
	                else if (sonars.hasHit(21) || sonars.hasHit(20)) {
	                    setRotationalVelocity(Math.PI / 8);
	                    setTranslationalVelocity(0.1);
	                } else if ((getCounter() % 100) == 0) {
	                    setRotationalVelocity(Math.PI / 2 * (0.5 - Math.random()));
	                    setTranslationalVelocity(0.5);
	                }
	            }
	        }
	    }

	
	/*   	  
	 * for test purposes
	 */
	public static void main(String[] args) 
	{	

		RoomLoaderDemo env = new RoomLoaderDemo("resources/room.png",true);
		//ImageLoaderEnvironment env = new ImageLoaderEnvironment("resources/room3.png",true);
		new Simbad(env,false);	
	}
}